#include <sys/select.h>
#include <stdio.h>
#include <unistd.h>
#include <stdlib.h>
#include <time.h>
#include <math.h>
#include "communication.h"
#include "control.h"

extern struct wheelspeed ws;
extern struct arm arm1;

int main(int argc,char **argv)
{
    int sockid;
    struct timeval s_time = {0,0};
    fd_set rset;

    if(argc != 3)
    {
        printf("Usage:%s [IP] [port]\r\n",argv[0]);
        exit(1);
    }

    sockid = connectToServer(argv[1],argv[2]);
    //初始化控制器参数
    initControl();
    //初始化机械臂末端的初始位置
    initArmEnd();

    //发送复位命令给webots，webots接收到后会将数据发送过来，在下面用readCom()接收数据
    sendCom(sockid,0,7,NULL,0);
    //终端输入目标点
    receiveTarget(1);
    while(1)
    {
        FD_ZERO(&rset);
        FD_SET(sockid,&rset);

        //监听网络
        if(select(sockid+1,&rset,NULL,NULL,&s_time)<=0)
            continue;
        readCom(sockid);

        //如果只是控制车体运动用下面一段
        /////////////////////////////////////////////////////////
        /*if(controlLog(sockid))
        {
            printf("congratulation,the car got to the destination\r\n");
            resetWheelSpeed();
            sendCom(sockid,0,9,(uint32_t *)&ws,4);
            receiveTarget(1); 
            continue;
        }*/
        //////////////////////////////////////////////////////////////

        //车体运动到物体附近然后视觉引导抓取 用这一段
        ///////////////////////////////////////////////////////////
        /*if(armWithPosControl(sockid))
        {
            printf("congratulation,the arm got to the destination\r\n");
            receiveTarget(1);
            continue;
        }*/
        ///////////////////////////////////////////////
        //printf("lf=%f lr=%f rf =%f rr=%f\r\n",ws.left_front,ws.left_rear,ws.right_front,ws.right_rear);

    }
}